#include "serial_transport.h"

// 串口有关数据
int Baudrate = 9600;			   // 波特率
string port_name = "/dev/ttyUSB1"; // 串口名字
unsigned char CtrlFlag = 1;		   // 发送信息的控制位

// 订阅/cmd_vel的控制信息
void handleMsg(const geometry_msgs::Twist::ConstPtr &msg, mySerial *s, unsigned char CtrlFlag)
{
	moveControl((float)msg->linear.x, (float)msg->angular.z, CtrlFlag, s);
	ROS_INFO("%2f,%f,%c\n", (float)msg->linear.x, (float)msg->angular.z, CtrlFlag);
}
int main(int argc, char **argv)
{

	///////// 发送的数据 /////////
	data_4 velocity; // 速度 m/s
	data_4 angle;	 // 角度 rad/s

	ros::init(argc, argv, "move_publish");
	ros::NodeHandle nh;

	// 串口对象
	mySerial ser(port_name, Baudrate);

	// 设置频率，10HZ
	ros::Rate loop_rate(10);
	geometry_msgs::Twist twist_msg;

	// 订阅/cmd_vel话题,并传给回调函数进行运动信息的收发
	ros::Subscriber sub = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, boost::bind(handleMsg, _1, &ser, CtrlFlag));
	// 等待100ms的时间
	loop_rate.sleep();
	ros::spin();

	return 0;
}
